package robocup.component.worldmodel.particlefilter;

import java.util.Random;

import robocup.component.geometry.Vektor;
import robocup.component.infotypes.FlagInfo;

public class ParticleFilter {

  private final Vektor particles[] = Vektor.getDummyInstances(100);

  private final Vektor particles2[] = Vektor.getDummyInstances(100);

  private int    validParticles = 0;

  private final Random random         = new Random(System.currentTimeMillis());

  private Region regionHelper   = null;

  private final Vektor vektorHelper   = new Vektor();

  /**
   * A class implementing the particle-filter as described in the UvA Master
   * Thesis. The filter is used for determin the players position.
   * 
   * This Particle Filter is more accurate but less efficient then the other. It
   * is therefore only used, when the ball is near as otherwise the accuracy is
   * of less importance.
   */
  public ParticleFilter() {

    super();

    this.validParticles = 0;
  }

  public void resetPosition(final Vektor pos) {

    if (pos == null) {
      this.validParticles = 0;
    }
    else {
      this.validParticles = 1;
      this.particles[0].x = pos.x;
      this.particles[0].y = pos.y;
    }
  }

  private boolean afterTrans    = false;

  private long    transCount    = 0;

  private long    nullpartCount = 0;

  public void addFlag(final FlagInfo fi,
      final double headAngle,
      final Vektor flagpos) {

    if (this.regionHelper == null) {
      this.regionHelper = new Region(fi, headAngle, flagpos);
    }
    else {
      this.regionHelper.reinitRegionFlag(fi, headAngle, flagpos);
    }

    if (this.validParticles != 0) {
      // compute mean
      final Vektor mean = this.vektorHelper;
      mean.x = 0;
      mean.y = 0;
      for (int i = 0; i < this.validParticles; i++) {
        mean.x += this.particles[i].x;
        mean.y += this.particles[i].y;
      }
      mean.x /= (double) this.validParticles;
      mean.y /= (double) this.validParticles;
      // compute standard deviation
      double dev = 0;
      for (int i = 0; i < this.validParticles; i++) {
        final double deltax = this.particles[i].x - mean.x;
        final double deltay = this.particles[i].y - mean.y;
        dev += (deltax * deltax + deltay * deltay);
      }
      dev /= (double) this.validParticles;
      dev = Math.sqrt(dev) / 12;
      dev *= Math.pow(1.0 / (double) this.validParticles, 1.0 / 6.0);
      // resample
      for (int i = this.validParticles; i < this.particles.length; i++) {
        final Vektor chosen = this.particles[this.random.nextInt(this.validParticles)];
        this.particles[i].x = chosen.x + (this.random.nextGaussian() * dev);
        this.particles[i].y = chosen.y + (this.random.nextGaussian() * dev);
      }
      this.validParticles = this.particles.length;
      int lastValidPlusOne = 0;
      for (int i = 0; i < this.validParticles; i++) {
        Vektor helper = null;
        if (this.regionHelper.insideRegion(this.particles[i]) == 1.0) {
          if (lastValidPlusOne != i) {
            helper = this.particles[i];
            this.particles[i] = this.particles[lastValidPlusOne];
            this.particles[lastValidPlusOne] = helper;
            // lastValidPlusOne++;
          }
          lastValidPlusOne++;
        }
      }
      this.validParticles = lastValidPlusOne;
    }
    if (this.afterTrans) {
      this.afterTrans = false;
      this.transCount++;
      if (this.validParticles == 0) {
        this.nullpartCount++;
      }
    }
    if (this.validParticles == 0) {
      for (int i = 0; i < this.particles.length; i++) {
        this.regionHelper.makeRandomInsideRegion(this.particles[i]);
      }
      this.validParticles = this.particles.length;
    }
  }

  public Vektor getPosition() {

    if (this.validParticles == 0) {
      return null;
    }
    final Vektor mean = this.vektorHelper;
    mean.x = 0;
    mean.y = 0;
    for (int i = 0; i < this.validParticles; i++) {
      mean.x += this.particles[i].x;
      mean.y += this.particles[i].y;
    }
    mean.x /= (double) this.validParticles;
    mean.y /= (double) this.validParticles;
    return mean.cloned();
  }

  public void translatePosition(final Vektor speed) {

    this.afterTrans = true;

    if (this.validParticles == 0) {
      return;
    }
    this.regionHelper.reinitRegionSpeed(speed, this.particles[0]);
    for (int i = 0; i < this.validParticles; i++) {
      this.regionHelper.setOrigin(this.particles[i]);
      this.regionHelper.makeRandomInsideRegion(this.particles2[i]);
    }
    
    // swap particles
    for(int i=0; i<this.particles.length; i++){
      final double x = particles[i].x;
      particles[i].x = particles2[i].x;
      particles2[i].x = x;
      
      final double y = particles[i].y;
      particles[i].y = particles2[i].y;
      particles2[i].y = y;
    }
  }

}
